from calendar import c
from re import I
import time
from turtle import goto
import cv2
import threading
import numpy as np
import rospy
import math
from functools import wraps
import sys
# 
from image_Tag_converter import ImgConverter
from image_Tag_converter import ImgConverter_head
from image_Tag_converter import TagConverter

sys.path.append("/home/lemon/catkin_ws/src/aelos_smart_ros")
from leju import base_action

robot_asciiArtName="""               ______ _      ____   _____       _____ __  __          _____ _______     ____   ____    __   ___                    
         /\   |  ____| |    / __ \ / ____|     / ____|  \/  |   /\   |  __ |__   __|   |  _ \ / __ \  / /  / _ \                   
        /  \  | |__  | |   | |  | | (___ _____| (___ | \  / |  /  \  | |__) | | |______| |_) | |  | |/ /_ | | | |                  
       / /\ \ |  __| | |   | |  | |\___ |______\___ \| |\/| | / /\ \ |  _  /  | |______|  _ <| |  | | '_ \| | | |                  
      / ____ \| |____| |___| |__| |____) |     ____) | |  | |/ ____ \| | \ \  | |      | |_) | |__| | (_) | |_| |                  
  ___/_/    \_|______|______\____/|_____/ _ __|_____/|_|  |_/_/   _\_|_|  \_\ |_|      |____/ \____/ \___/_\___/____               
 |  __ \                                 | |  _ \         | |    (_\ \   / /                  ___      / ____|  ____|              
 | |__) |   _____      _____ _ __ ___  __| | |_) |_   _   | |     _ \ \_/ __ _ _ __   __ _   ( _ )    | (___ | |__                 
 |  ___/   / _ \ \ /\ / / _ | '__/ _ \/ _` |  _ <| | | |  | |    | | \   / _` | '_ \ / _` |  / _ \/\   \___ \|  __|                
 | |      | (_) \ V  V |  __| | |  __| (_| | |_) | |_| |  | |____| |  | | (_| | | | | (_| | | (_>  <   ____) | |                   
 |_|       \___/ \_/\_/ \___|_|  \___|\__,_|____/ \__, |  |______|_|  |_|\__,_|_| |_|\__, |  \___/\/  |_____/|_|                   
                                                   __/ |                              __/ |                                        
                                                  |___/                              |___/                                         """
# ******************************************定义一些参数***********************************************
chest_circle_x = None
chest_circle_y = None
Chest_img = None
Head_img = None
ChestOrg = None
HeadOrg = None
marker = None
Debug = 0


# ******************************************不同色块的HSV范围***********************************************
color_range = {
    'green': [( 66, 99 , 91 ),( 100 , 165 , 204 )],
    'orange': [( 17 , 243 , 125 ),(20 ,255 , 176 )],
    'upgreen': [( 77 , 87 , 127 ),(87 ,104 , 219 )],
    'uporange': [( 21 , 142 , 106 ),(28 ,182 , 138 )]
}

# ******************************************动作函数***********************************************
#独立动作#
def Box_Down(n):             #放下箱子
    for i in range (0,n):
        base_action.action("PutCubeDown")
        time.sleep(0.5)
def Box_Stand(n):
    for i in range (0,n):
        base_action.action("stableAction_3") #寻找适合自己的抱箱动作初始零点 #TODO
        time.sleep(0.5)
# 抓取找寻最佳 #TODO
def Box_Grab(n):            #正常小抓
    for i in range (0,n):
        base_action.action("grabCube1_05")
        #base_action.action("grabCube1_03")
        time.sleep(0.5)
def Box_Grab1(n):           #正常大抓                  #FINISH
    for i in range (0,n):
        base_action.action("grabCube1_05")
        #base_action.action("grabCube1_03")
        time.sleep(0.5)
def Box_Grab2(n):
    for i in range (0,n):   #蹲下大抓
        base_action.action("grabCube1_05")
        #base_action.action("grabCube1_03")
        time.sleep(0.5)
# TODO
def Box_Lift(n):
    for i in range (0,n):
        base_action.action("liftCubeUp_11")
        time.sleep(0.5)
def Stand(n): #寻找适合自己的非抱箱动作初始零点 #TODO
    for i in range (0,n):
        base_action.action("stand1") 
        time.sleep(0.5)
def Body_Down(n):           #摔倒                      #FINISH
    for i in range (0,n):
        base_action.action("actor_down")
        time.sleep(0.5)

#后退#
def Box_Back(n):
    for i in range (0,n):    #抱箱后退                  #FINISH
            base_action.action("boxback2Run_7") #好
            time.sleep(0.5)
def Box_Back1(n):            #这个用不到，暂且保留
    for i in range (0,n):
            base_action.action("BoxBack2-1")
            time.sleep(0.5)
def Back2(n):                #不抱箱后退                #FINISH
    for i in range (0,n):
            base_action.action("back2Run_8")
            time.sleep(0.5)
def Back1(n):
    for i in range (0,n):
            base_action.action("back1Run_8")
            time.sleep(0.5)
#前进#
def go_fast(n):              #缓慢前进一步               #FINISH
    for i in range (0,n):
        base_action.action("Forwalk01")
        time.sleep(0.5)
def go_fast1(n):             #快速前进一步               #FINISH
    for i in range (0,n):
        base_action.action("FastForward1s")
        time.sleep(0.5)
def go_fast2(n):             #快速前进两步               #FINISH
    for i in range (0,n):
        base_action.action("FastForward2s")
        time.sleep(0.5)
def go_fast3(n):             #快速前进三步               #FINISH
    for i in range (0,n):
        base_action.action("FastForward3s")
        time.sleep(0.5)
def go_fast4(n):             #快速前进四步               #FINISH
    for i in range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
def go_fast5(n):             #快速前进五步
    for i in range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
def go_fast6(n):             #快速前进六步
    for i in range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
def box_go(n):               #抱箱缓慢前进一步           #FINISH
    for i in range (0,n):
        base_action.action("BoxForward02_4")
        time.sleep(0.5)
def box_go1(n):              #抱箱快速前进一步           #FINISH
    for i in range (0,n):
        base_action.action("BoxForward1s")
        time.sleep(1)
def box_go2(n):              #抱箱快速前进两步           #FINISH
    for i in range (0,n):
        base_action.action("BoxForward2s")
        time.sleep(1)
def box_go3(n):              #抱箱快速前进三步           #FINISH
    for i in range (0,n):
        box_go1(3)
        # base_action.action("boxForward3s") #这个貌似没有
        time.sleep(1)
def box_go4(n):              #抱箱快速前进四步
    print("go4")
    for i in range (0,n):
        base_action.action("BoxForward4s_5")
        time.sleep(0.5)
def box_go5(n):              #抱箱快速前进五步
    for i in range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
def box_go6(n):              #抱箱快速前进六步
    for i in range (0,n):
        base_action.action("BoxForward6s_5")
        time.sleep(0.5)
def box_go8(n):              #抱箱快速前进六步
    for i in range (0,n):
        base_action.action("boxForward8s_B081_5")
        time.sleep(0.5)
def Box_go_slow4s (n):           #右转
    for i in  range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
def Box_go_slow6s (n):           #右转
    for i in  range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
def Box_go_slow8s (n):           #右转
    for i in  range (0,n):
        #TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO#TODO
        time.sleep(0.5)
#侧移#
def L_move1 (n):             #小左侧移              #FINISH
    for i in  range (0,n):
        #base_action.action ("Left02move2")
        base_action.action ("Left02move")
        time.sleep(0.5)
def R_move1 (n):             #小右侧移              #FINISH
    for i in  range (0,n):
        #base_action.action ("Right02move2")
        base_action.action ("Right02move")
        time.sleep(0.5)
def L_move2 (n):             #大左侧移              #FINISH
    for i in  range (0,n):
        base_action.action('moveLeft2_23')
        time.sleep(0.2)
def R_move2 (n):             #大右侧移              #FINISH
    for i in  range (0,n):
        base_action.action('moveRight2_10')
        time.sleep(0.2)
def BoxL_move1 (n):          #抱箱小左侧移           #FINISH
    for i in  range (0,n):
        base_action.action ("BoxLeft1")
        time.sleep(1)
def BoxL_move2 (n):          #抱箱大左侧移           #FINISH
    for i in  range (0,n):
        base_action.action ("boxMoveLeft2_21_10")
        time.sleep(0.2)
def BoxR_move1 (n):          #抱箱小右侧移           #FINISH
    for i in  range (0,n):
        base_action.action ("BoxRight1")
        time.sleep(1)
def BoxR_move2 (n):          #抱箱大右侧移           #FINISH
    for i in  range (0,n):
        base_action.action ("boxMoveRight55")
        time.sleep(0.2)
#转向#
def L_turn1 (n):             #小左转              #FINISH
    for i in  range (0,n):
        base_action.action("TurnL1_7")
        # base_action.action("turn003L")
        # base_action.action("TurnL1_m1")
        # base_action.action("TurnL1_m2")
        # base_action.action("TurnL1_m3")
        # base_action.action("TurnL1_m4")
        time.sleep(0.5)
def R_turn1 (n):             #小右转              #FINISH
    for i in  range (0,n):
        base_action.action("TurnR1_12")
        # base_action.action("TurnR1_m1")
        # base_action.action("TurnR1_m2")
        # base_action.action("TurnR1_m3")
        # base_action.action("TurnR1_m4")
        time.sleep(0.5)
def L_turn2 (n):             #大左转              #FINISH
    for i in range (0,n):
        base_action.action("leftTurn2_3")
        # base_action.action("LeftTurn1s")
        time.sleep(0.5)
def R_turn2 (n):             #大右转              #FINISH
    for i in  range (0,n):
        base_action.action("rightTurn2_3")
        # base_action.action("RightTurn1s")
        time.sleep(0.5)
def BoxL_turn1(n):           #抱箱小左转 
    for i in  range (0,n):
        #base_action.action("boxTurnL1_14")#去刚度加延迟目前还行
        # base_action.action("boxTurnL1_11")#只加延迟
        base_action.action("boxTurnL1_test_m11")
        base_action.action("boxTurnL1_test_m21")
        base_action.action("boxTurnL1_test_m31")
        base_action.action("boxTurnL1_test_m41")
        time.sleep(1)
def BoxR_turn1 (n):          #抱箱小右转
    for i in  range (0,n):
        #base_action.action("boxTurnR1_14")#去刚度加延迟目前还行
        # base_action.action("boxTurnR1_11")#只加延迟
        base_action.action("boxTurnR1_test_m11")
        base_action.action("boxTurnR1_test_m21")  
        base_action.action("boxTurnR1_test_m31")
        base_action.action("boxTurnR1_test_m41")
        time.sleep(1)
def BoxL_turn2(n):           #抱箱大左转           #FINISH
    for i in  range (0,n):
        base_action.action("BoxLeftTurn1s")
        time.sleep(1)
def BoxR_turn2 (n):          #抱箱大右转           #FINISH
    print("右大转")
    for i in  range (0,n):
        base_action.action("BoxRightTurn1s")
        time.sleep(1)


# ******************************************图像识别***********************************************
#获取胸部图像
def get_img():
    print("---------执行了get_img---------")
    global Chest_img,ChestOrg
    global ret
    image_reader_chest = ImgConverter()
    while True:
        ret, ChestOrg = image_reader_chest.chest_image()
        time.sleep(0.3)
        if ChestOrg is not None:
            Chest_img = ChestOrg
            time.sleep(0.05)
            #Chest_img = cv2.flip(Chest_img, 1)
        else:
            time.sleep(0.3)
            print("暂时未获取到图像")
th2 = threading.Thread(target=get_img)
th2.setDaemon(True)
th2.start()

#获取头部图像
def get_img_head():
    print("---------执行了get_img_head---------")
    global Head_img,HeadOrg
    global ret
    image_reader_head = ImgConverter_head()
    while True:
        ret, HeadOrg = image_reader_head.head_image()
        time.sleep(0.3)
        if HeadOrg is not None:
            Head_img = HeadOrg
            time.sleep(0.05)
            #Chest_img = cv2.flip(Chest_img, 1)
        else:
            time.sleep(0.3)
            print("暂时未获取到图像")
th3 = threading.Thread(target=get_img_head)
th3.setDaemon(True)
th3.start()

#查找方块
def find_box(img,color_name):
    print("---------执行了find_box---------")
    global chest_circle_x, chest_circle_y ,p
    if Chest_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('A','x=',chest_circle_x,'y=',chest_circle_y)
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)

        else:
            print('正在寻找目标')
def find_nearest_box() :
    print("---------执行了find_nearest_box---------")
    global chest_circle_x, chest_circle_y
    is_find_over = False
    is_find_green = False
    is_find_orange = False
    img = Chest_img 
    if Chest_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        color_name ='green' #绿色方块
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('Green','x=',chest_circle_x,'y=',chest_circle_y)
            if   chest_circle_x>90 and chest_circle_y>180:
                if   chest_circle_y>350 :
                    if chest_circle_x<600:
                        is_find_green = True
                        print("找到绿色")
                elif chest_circle_y>260 and chest_circle_y<350 :
                    if chest_circle_x<600:
                        is_find_green = True
                        print("找到绿色")
                elif chest_circle_y>200 and chest_circle_y<260 :
                    if chest_circle_x>250 and chest_circle_x<575:
                        is_find_green = True
                        print("找到绿色")
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)
        else:
            print("关于绿色的什么都没发现")

        color_name ='orange' #绿色方块
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('Orange','x=',chest_circle_x,'y=',chest_circle_y)
            if   chest_circle_x>90 and chest_circle_y>180:
                if   chest_circle_y>350 :
                    if chest_circle_x<600:
                        is_find_orange = True
                        print("找到橙色")
                elif chest_circle_y>260 and chest_circle_y<350 :
                    if chest_circle_x<600:
                        is_find_orange = True
                        print("找到橙色")
                elif chest_circle_y>200 and chest_circle_y<260 :
                    if chest_circle_x>250 and chest_circle_x<575:
                        is_find_orange = True
                        print("找到橙色")
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)
        else:
            print("关于红色的什么都没发现")
    if is_find_green == True or is_find_orange == True:
        is_find_over = True
    return is_find_over

def find_box_green() :
    print("---------执行了find_box_green---------")
    global chest_circle_x, chest_circle_y
    is_find_over = False
    is_find_green = False
    img = Chest_img 
    if Chest_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        color_name ='green' #绿色方块
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('Green','x=',chest_circle_x,'y=',chest_circle_y)
            if   chest_circle_x>90 and chest_circle_y>180:
                if   chest_circle_y>350 :
                    if chest_circle_x<600:
                        is_find_green = True
                        print("找到绿色")
                elif chest_circle_y>260 and chest_circle_y<350 :
                    if chest_circle_x<600:
                        is_find_green = True
                        print("找到绿色")
                elif chest_circle_y>200 and chest_circle_y<260 :
                    if chest_circle_x>250 and chest_circle_x<575:
                        is_find_green = True
                        print("找到绿色")
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)
        else:
            print("关于绿色的什么都没发现")
    if is_find_green == True :
        is_find_over = True
    return is_find_over
def find_box_orange() :
    print("---------执行了find_box_orange---------")
    global chest_circle_x, chest_circle_y
    is_find_over = False
    is_find_orange = False
    img = Chest_img 
    if Chest_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        color_name ='orange' #绿色方块
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('Orange','x=',chest_circle_x,'y=',chest_circle_y)
            if   chest_circle_x>90 and chest_circle_y>180:
                if   chest_circle_y>350 :
                    if chest_circle_x<600:
                        is_find_orange = True
                        print("找到橙色")
                elif chest_circle_y>260 and chest_circle_y<350 :
                    if chest_circle_x<600:
                        is_find_orange = True
                        print("找到橙色")
                elif chest_circle_y>200 and chest_circle_y<260 :
                    if chest_circle_x>250 and chest_circle_x<575:
                        is_find_orange = True
                        print("找到橙色")
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)
        else:
            print("关于红色的什么都没发现")
    if is_find_orange == True:
        is_find_over = True
    return is_find_over
def find_Headbox_orange() :
    print("---------执行了find_Headbox_orange---------")
    global chest_circle_x, chest_circle_y 
    is_find_over = False
    is_find_orange = False
    img = Head_img 
    if Head_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        color_name ='uporange' #绿色方块
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('Orange','x=',chest_circle_x,'y=',chest_circle_y)
            if chest_circle_x>280 and chest_circle_x<450 and chest_circle_y>290 and chest_circle_y<450:
                is_find_orange = True
                print("红色还在胸口")
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)
        else:
            print("关于红色的什么都没发现")
    if is_find_orange == True :
        is_find_over = True
    return is_find_over
def find_Headbox_green() :
    print("---------执行了find_Headbox_green---------")
    global chest_circle_x, chest_circle_y 
    is_find_over = False
    is_find_green = False
    img = Head_img 
    if Head_img is None:
        print('等待获取图像中...')
        time.sleep(0.3)
    else:
        color_name ='upgreen' #绿色方块
        box_img = img
        box_img_bgr = cv2.cvtColor(box_img, cv2.COLOR_RGB2BGR)  # 将图片转换到BRG空间
        box_img_hsv = cv2.cvtColor(box_img, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        box_img = cv2.GaussianBlur(box_img_hsv, (3, 3), 0)  # 高斯模糊
        box_img_mask = cv2.inRange(box_img, color_range[color_name][0], color_range[color_name][1])  # 二值化
        box_img_closed = cv2.erode(box_img_mask, None, iterations=2)  # 腐蚀
        box_img_opened = cv2.dilate(box_img_mask, np.ones((4, 4), np.uint8), iterations=2)  # 膨胀    先腐蚀后运算等同于开运算
        (contours, hierarchy) = cv2.findContours(box_img_opened, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if len(contours) != 0:
            area = []
            for cn in contours:
                contour_area = math.fabs(cv2.contourArea(cn))
                area.append(contour_area)
            max_index = np.argmax(area)
            (chest_circle_x,chest_circle_y),chest_radius = cv2.minEnclosingCircle(contours[max_index])
            cv2.circle(img,(int(chest_circle_x),int(chest_circle_y)),int(chest_radius),(0,0,255))
            print('Green','x=',chest_circle_x,'y=',chest_circle_y)
            if chest_circle_x>280 and chest_circle_x<450 and chest_circle_y>290 and chest_circle_y<450:
                is_find_green = True
                print("绿色还在胸口")
            if Debug:
                # cv2.imwrite('image.png',img)
                cv2.imshow("Box", img)
                cv2.waitKey(2000)
        else:
            print("关于绿色的什么都没发现")
    if is_find_green == True :
        is_find_over = True
    return is_find_over
#搬箱子
def goto_box():
    print("---------执行了goto_box---------")
    global level, ID
    if chest_circle_x is None :
        print('等待中获取坐标中...')
        time.sleep(0.3)
    else:
        if chest_circle_x < 300:
            print("正在左侧移 ",chest_circle_x)
            L_move2(1)
            time.sleep(0.2)
            if chest_circle_x>80:
                if chest_circle_y < 300:  
                    print("前进2",chest_circle_y)
                    go_fast1(1)
                    time.sleep(0.2)
                elif chest_circle_y < 380:
                    go_fast(1)
                    time.sleep(0.2)
                    print("前进1",chest_circle_y)
        elif chest_circle_x > 395:    
           print("正在右侧移 ",chest_circle_x)
           R_move2(1)
           time.sleep(0.2)
        else:
            if chest_circle_y < 300:  
                print("前进2",chest_circle_y)
                go_fast1(1)
                time.sleep(0.2)
            elif chest_circle_y < 380:
                go_fast(1)
                time.sleep(0.2)
                print("前进1",chest_circle_y)
            else:
                print("开始抱箱子")
                Box_Grab(1)
                Box_Lift(1)
                level = "end_box"

def goto_box_plus():
    print("---------执行了goto_box_plus---------")
    global level, ID
    if chest_circle_x is None :
        print('等待中获取坐标中...')
        time.sleep(0.3)
    else:
        if chest_circle_x < 300:
            print("正在左侧移 ",chest_circle_x)
            L_move2(1)
            time.sleep(0.2)
            if chest_circle_x>80:
                if chest_circle_y < 300:  
                    print("前进2",chest_circle_y)
                    go_fast1(1)
                    time.sleep(0.2)
                elif chest_circle_y < 350:
                    go_fast(1)
                    time.sleep(0.2)
                    print("前进1",chest_circle_y)
        elif chest_circle_x > 395:    
           print("正在右侧移 ",chest_circle_x)
           R_move2(1)
           time.sleep(0.2)
        else:
            if chest_circle_y < 300:  
                print("前进2",chest_circle_y)
                go_fast1(1)
                time.sleep(0.2)
            elif chest_circle_y < 350:
                go_fast(1)
                time.sleep(0.2)
                print("前进1",chest_circle_y)
            else:
                print("开始抱箱子")
                Box_Grab(1)
                Box_Lift(1)
                level = "end_box"
def goto_box_mini():
    print("---------执行了goto_box_mini---------")
    global level, ID
    if chest_circle_x is None :
        print('等待中获取坐标中...')
        time.sleep(0.3)
    else:
        if chest_circle_x < 260:
            print("正在左侧移 ",chest_circle_x)
            L_move2(1)
            time.sleep(0.2)
            if chest_circle_x>80:
                if chest_circle_y < 300:  
                    print("前进2",chest_circle_y)
                    go_fast1(1)
                    time.sleep(0.2)
                elif chest_circle_y < 350:
                    go_fast(1)
                    time.sleep(0.2)
                    print("前进1",chest_circle_y)
        elif chest_circle_x > 395:    
           print("正在右侧移 ",chest_circle_x)
           R_move2(1)
           time.sleep(0.2)
        else:
            if chest_circle_y < 300:  
                print("前进2",chest_circle_y)
                go_fast1(1)
                time.sleep(0.2)
            elif chest_circle_y < 350:
                go_fast(1)
                time.sleep(0.2)
                print("前进1",chest_circle_y)
            else:
                print("开始抱箱子")
                Box_Grab2(1)
                Box_Lift(1)
                level = "end_box"
def goto_box_plus_pro(color_name):
    print("---------执行了goto_box_plus_pro---------")
    global level, ID
    True_Box_Sum=0
    if chest_circle_x is None :
        print('等待中获取坐标中...')
        time.sleep(0.3)
    else:
        if chest_circle_x < 300:
            print("正在左侧移 ",chest_circle_x)
            L_move2(1)
            time.sleep(0.2)
            if chest_circle_x>80:
                if chest_circle_y < 300:  
                    print("前进2",chest_circle_y)
                    go_fast1(1)
                    time.sleep(0.2)
                elif chest_circle_y < 350:
                    go_fast(1)
                    time.sleep(0.2)
                    print("前进1",chest_circle_y)
        elif chest_circle_x > 395:    
           print("正在右侧移 ",chest_circle_x)
           R_move2(1)
           time.sleep(0.2)
        else:
            if chest_circle_y < 300:  
                print("前进2",chest_circle_y)
                go_fast1(1)
                time.sleep(0.2)
            elif chest_circle_y < 350:
                go_fast(1)
                time.sleep(0.2)
                print("前进1",chest_circle_y)
            else:
                print("开始抱箱子")
                Box_Grab(1)
                Box_Lift(1)
                if color_name=='upgreen':
                    result_box_1=find_Headbox_green()
                    if result_box_1==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T1")
                    time.sleep(0.5)
                    result_box_2=find_Headbox_green()
                    if result_box_2==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T2")
                    time.sleep(0.5)
                    result_box_3=find_Headbox_green()
                    if result_box_3==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T3")
                    time.sleep(0.5)
                    result_box_4=find_Headbox_green()
                    if result_box_4==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T4")
                    time.sleep(0.5)
                    result_box_5=find_Headbox_green()
                    if result_box_5==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T5")
                elif color_name=='uporange':
                    result_box_1=find_Headbox_orange()
                    if result_box_1==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T1")
                    time.sleep(0.5)
                    result_box_2=find_Headbox_orange()
                    if result_box_2==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T2")
                    time.sleep(0.5)
                    result_box_3=find_Headbox_orange()
                    if result_box_3==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T3")
                    time.sleep(0.5)
                    result_box_4=find_Headbox_orange()
                    if result_box_4==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T4")
                    time.sleep(0.5)
                    result_box_5=find_Headbox_orange()
                    if result_box_5==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T5")
                if True_Box_Sum>3 or True_Box_Sum==3:
                    level = "end_box"
                else:
                    Stand(1)
                    time.sleep(0.2)
def goto_box_mini_pro(color_name):
    print("---------执行了goto_box_mini_pro---------")
    global level, ID
    True_Box_Sum=0
    if chest_circle_x is None :
        print('等待中获取坐标中...')
        time.sleep(0.3)
    else:
        if chest_circle_x < 275:
            print("正在左侧移 ",chest_circle_x)
            L_move2(1)
            time.sleep(0.2)
            if chest_circle_x>80:
                if chest_circle_y < 300:  
                    print("前进2",chest_circle_y)
                    go_fast1(1)
                    time.sleep(0.2)
                elif chest_circle_y < 350:
                    go_fast(1)
                    time.sleep(0.2)
                    print("前进1",chest_circle_y)
        elif chest_circle_x > 350:    
           print("正在右侧移 ",chest_circle_x)
           R_move2(1)
           time.sleep(0.2)
        else:
            if chest_circle_y < 300:  
                print("前进2",chest_circle_y)
                go_fast1(1)
                time.sleep(0.2)
            elif chest_circle_y < 350:
                go_fast(1)
                time.sleep(0.2)
                print("前进1",chest_circle_y)
            else:
                print("开始抱箱子")
                Box_Grab1(1)
                Box_Lift(1)
                if color_name=='upgreen':
                    result_box_1=find_Headbox_green()
                    if result_box_1==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T1")
                    time.sleep(0.5)
                    result_box_2=find_Headbox_green()
                    if result_box_2==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T2")
                    time.sleep(0.5)
                    result_box_3=find_Headbox_green()
                    if result_box_3==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T3")
                    time.sleep(0.5)
                    result_box_4=find_Headbox_green()
                    if result_box_4==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T4")
                    time.sleep(0.5)
                    result_box_5=find_Headbox_green()
                    if result_box_5==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T5")
                elif color_name=='uporange':
                    result_box_1=find_Headbox_orange()
                    if result_box_1==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T1")
                    time.sleep(0.5)
                    result_box_2=find_Headbox_orange()
                    if result_box_2==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T2")
                    time.sleep(0.5)
                    result_box_3=find_Headbox_orange()
                    if result_box_3==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T3")
                    time.sleep(0.5)
                    result_box_4=find_Headbox_orange()
                    if result_box_4==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T4")
                    time.sleep(0.5)
                    result_box_5=find_Headbox_orange()
                    if result_box_5==True:
                        True_Box_Sum=True_Box_Sum+1
                        print("T5")
                if True_Box_Sum>3 or True_Box_Sum==3:
                    level = "end_box"
                else:
                    Stand(1)
                    time.sleep(0.2)
#tag对正
def turn_to_tag(dis_x, dis_y, theta, x_offset=0, y_offset=0, theta_offset=0, x_threshold=0.03, y_threshold=0.07, theta_threshold=5):
    is_turn_done = False
    check_flag = 0
    x_error = dis_x-x_offset
    y_error = dis_y-y_offset
    theta_error = theta-theta_offset
    print("---------执行了turn_to_tag---------")
    print("ID:", marker[0], "dis_x:", dis_x, "dis_y:", dis_y, "theta:", theta, "theta_offset", theta_offset)
    print("x_error:", x_error, "y_error:", y_error, "theta_error:", theta_error)
    # 不偏离赛道
    if step == 1:
        if (x_error < x_threshold-0.03):
            print("后退",x_error, " < ", x_threshold-0.03)
            Box_Back(1)
        elif (theta_error+theta_threshold > 25):
            print("1左转身", theta_error, " > ", 14+theta_threshold)
            BoxL_turn2(1)
        elif (theta_error-theta_threshold < -25):
            print("1右转身", theta_error, " < ", -14-theta_threshold)
            BoxR_turn2(1)
        # 左右平移
        elif (y_error > y_threshold):
            print("2左移动", y_error, " > ", y_threshold)
            BoxL_move2(1)
        elif (y_error < -y_threshold):
            print("2右移动 ", y_error," < ", -y_threshold)
            BoxR_move2(1)
        # 快速靠近标签
        elif (x_error > x_threshold+0.25):
            print("向前靠近标签", x_error, " > ", x_threshold+0.25)
            box_go2(1)
        # 方向平行与标签
        elif (theta_error > theta_threshold):
            print("2左转 ", theta_error, " > ", theta_threshold)
            BoxL_turn1(1)
        elif (theta_error < -theta_threshold):
            print("2右转 ", theta_error, " < ", theta_threshold)
            BoxR_turn1(1)
        elif (x_error > x_threshold+0.04):
            print("向前靠近标签", x_error, " > ", x_threshold+0.1)
            box_go(1)
        else:
            Box_Stand(1)
            is_turn_done = True 
    elif step == 2:
        if (x_error < x_threshold-0.03):
            print("后退",x_error, " < ", x_threshold-0.03)
            Back2(1)
        elif (theta_error+theta_threshold > 25):
            print("1左转身", theta_error, " > ", 14+theta_threshold)
            L_turn2(1)
        elif (theta_error-theta_threshold < -25):
            print("1右转身", theta_error, " < ", -14-theta_threshold)
            R_turn2(1)
        # 左右平移
        elif (y_error > y_threshold):
            print("1左移动", y_error, " > ", y_threshold)
            L_move2(1)
        elif (y_error < -y_threshold):
            print("1右移动 ", y_error," < ", -y_threshold)
            R_move2(1)
        # 快速靠近标签
        elif (x_error > x_threshold+0.25):
            print("向前靠近标签", x_error, " > ", x_threshold+0.12)
            go_fast2(1)
        # 方向平行与标签
        elif (theta_error > theta_threshold):
            print("2左转 ", theta_error, " > ", theta_threshold)
            L_turn1(1)
        elif (theta_error < -theta_threshold):
            print("2右转 ", theta_error, " < ", theta_threshold)
            R_turn1(1)
        # 靠近标签
        elif (x_error > x_threshold+0.12):
            print("向前靠近标签", x_error, " > ", x_threshold+0.08)
            go_fast1(1)
        elif (x_error > x_threshold+0.06):
            print("向前靠近标签", x_error, " > ", x_threshold+0.06)
            go_fast(1)
        else:    
            is_turn_done = True 

    return is_turn_done
def turn_to_angle(theta,theta_offset=0,theta_threshold=5):
    is_turn_done = False
    check_flag = 0
    theta_error = theta-theta_offset
    print("---------执行了turn_to_angle---------")
    print("ID:", "theta:", theta, "theta_offset", theta_offset)
    print("theta_error:", theta_error)
    if step == 1:
        if   (theta_error+theta_threshold > 25):
            print("1左转身", theta_error, " > ", 14+theta_threshold)
            BoxL_turn2(1)
        elif (theta_error-theta_threshold < -25):
            print("1右转身", theta_error, " < ", -14-theta_threshold)
            BoxR_turn2(1)
        elif (theta_error > theta_threshold):
            print("2左转 ", theta_error, " > ", theta_threshold)
            BoxL_turn1(1)
        elif (theta_error < -theta_threshold):
            print("2右转 ", theta_error, " < ", theta_threshold)
            BoxR_turn1(1)
        else:
            Box_Stand(1)
            is_turn_done = True 
    elif step == 2:
        if (theta_error+theta_threshold > 25):
            print("1左转身", theta_error, " > ", 14+theta_threshold)
            L_turn2(1)
        elif (theta_error-theta_threshold < -25):
            print("1右转身", theta_error, " < ", -14-theta_threshold)
            R_turn2(1)
        # 方向平行与标签
        elif (theta_error > theta_threshold):
            print("2左转 ", theta_error, " > ", theta_threshold)
            L_turn1(1)
        elif (theta_error < -theta_threshold):
            print("2右转 ", theta_error, " < ", theta_threshold)
            R_turn1(1)
        else:    
            is_turn_done = True 

    return is_turn_done

def turn_to_y(dis_y, y_offset=0,y_threshold=0.07):
    is_turn_done = False
    check_flag = 0
    y_error = dis_y-y_offset
    print("---------执行了turn_to_y---------")
    print("ID:", marker[0],"dis_y:", dis_y)
    print("y_error:", y_error)
    # 不偏离赛道
    if step == 1:
        # 左右平移
        if   (y_error > y_threshold):
            print("2左移动", y_error, " > ", y_threshold)
            BoxL_move2(1)
        elif (y_error < -y_threshold):
            print("2右移动 ", y_error," < ", -y_threshold)
            BoxR_move2(1)
        else:
            Box_Stand(1)
            is_turn_done = True 
    elif step == 2:
        # 左右平移
        if   (y_error > y_threshold):
            print("1左移动", y_error, " > ", y_threshold)
            L_move2(1)
        elif (y_error < -y_threshold):
            print("1右移动 ", y_error," < ", -y_threshold)
            R_move2(1)
        else:    
            is_turn_done = True 

    return is_turn_done

def turn_to_x(dis_x , x_offset=0 , x_threshold=0.03):
    is_turn_done = False
    check_flag = 0
    x_error = dis_x-x_offset
    print("---------执行了turn_to_x---------")
    print("ID:", marker[0], "dis_x:", dis_x)
    print("x_error:", x_error)
    if step == 1:
        if (x_error < x_threshold-0.03):
            print("后退",x_error, " < ", x_threshold-0.03)
            Box_Back(1)
        elif (x_error > x_threshold+0.25):
            print("向前靠近标签", x_error, " > ", x_threshold+0.25)
            box_go2(1)
        elif (x_error > x_threshold+0.04):
            print("向前靠近标签", x_error, " > ", x_threshold+0.1)
            box_go(1)
        else:
            Box_Stand(1)
            is_turn_done = True 
    elif step == 2:
        if (x_error < x_threshold-0.03):
            print("后退",x_error, " < ", x_threshold-0.03)
            Back2(1)
        else:    
            is_turn_done = True 

    return is_turn_done

if __name__ == '__main__':
    rospy.init_node('image_listener')            #ROS节点初始化
    Tag = TagConverter()
    time.sleep(0.5)
    ID = 0      
    step = 1
    tag1k=0
    level = "start_box"
    time.sleep(5)
    print("#############################################开始获取胸部摄像头#############################################")
    while ChestOrg is None:
        print('等待获取胸部摄像头')
        time.sleep(0.1)
    print(robot_asciiArtName)
    while True:                                  #--------------------------------------大循环,直到代码结束--------------------------------------#
        Stand(1)
        time.sleep(0.1)
        print('启动成功')
        print("#############################################开始确定启动参数#############################################")
        while True:                              #-----------------------------------------确定启动参数-----------------------------------------#
            print("颜色代号: r -->红色 g-->绿色")
            print("方案代号: 1 -->启动 2-->直接搬 3-->靠墙 4-->对墙边界")
            key_color=input("请输入颜色代号: ")
            key_case=input("请输入方案代号: ")
            key_ifIdentifyTag1 = None
            if key_case == "2":
                print("方案代号: 1 -->识别tag1 2-->不识别tag1 other-->自动判断")
                key_ifIdentifyTag1 = input("是否识别1号tag码")
            print("1 -->确定 other: 重写")
            key_okey=input("确定or重写: ")
            if key_okey=="1":
                break
        print("#############################################开始做方案启动动作#############################################")
        if   key_case=="1":                      #-----------------------------------------确定启动动作-----------------------------------------#
            print("1-->初始方块启动动作")
            L_move2(4)
        elif key_case=="2":
            print("2-->直接搬启动动作")
            # Box_Grab(1)
            Box_Grab2(1)
            Box_Lift(1)
            level = "end_box"
        elif key_case=="3":
            print("3-->靠墙启动动作")
            L_move2 (3)
        elif key_case=="4":
            print("4-->对墙边界启动动作")
            R_move2(3)
        print("#############################################开始抱箱和运输操作#############################################")
        while not rospy.is_shutdown():           #-----------------------------------------进入运行循环-----------------------------------------#
            if ID == 0:  #搬箱子和搬完箱子前往tag1的准备
                if level == "start_box":#搬箱子
                    print("********************寻找箱子********************")
                    if   key_color=="g":
                        up_color_name='upgreen'
                        find_box(Chest_img, 'green')
                    elif key_color=="r":
                        up_color_name='uporange'
                        find_box(Chest_img, 'orange')
                    else:
                        up_color_name='upgreen'
                        find_box(Chest_img, 'green')
                    print("********************尝试进行抱箱********************")
                    if   key_case=="3":
                        # goto_box_mini_pro(up_color_name)
                        goto_box_mini()
                    elif key_case=="1":
                        # goto_box_plus_pro(up_color_name)
                        goto_box_mini()
                    else:
                        goto_box_mini()
                    time.sleep(0.1)
                elif level == "end_box":
                    print("********************执行前往1号tag的准备动作********************")
                    if   key_case=="1":
                        print("case 1 搬箱前往tag")
                        # BoxL_turn1(1)
                        BoxL_move2(4)
                        box_go4(1)
                        # Box_go_slow4s(1)
                    elif key_case=="2":
                        print("case 2 搬箱前往tag")
                        # BoxL_turn1(2)
                        BoxL_move2(4)
                        BoxL_move2(5)
                    elif key_case=="3":
                        print("case 3 搬箱前往tag")
                        BoxL_turn1(2)
                        Box_Back(1)
                        BoxL_move2(4)
                    elif key_case=="4":
                        print("case 1 搬箱前往tag")
                        BoxR_turn2(6) #右转
                        BoxR_turn1(2)
                        box_go(3)
                    ID+=1
                elif level == "next_box":
                    print("********************执行再次搬箱的准备动作********************")
                    while True:
                        if   homing_level == "homing_1" :                        #第一步转身找码
                            L_turn2(1)
                            marker = Tag.get_nearest_marker()
                            if len(marker) != 0:
                                L_turn2(2)
                                homing_level = "homing_2"
                                print("找到回归tag码")
                        elif homing_level == "homing_2" :                        #第二步精确对码
                            while True:
                                marker = Tag.get_nearest_marker()
                                if len(marker) != 0:
                                    robot_tag_x = marker[1]
                                    robot_tag_y = marker[2]
                                    tag_yaw = marker[3] + 90
                                    break
                            result=turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.25 , 0.1 , -8 , 0.03 , 0.07 , 2 )
                            # result=turn_to_x(robot_tag_x,0.25)
                            if result == True:
                                homing_level = "homing_3"
                                print("回归tag码对准完毕")
                        elif homing_level == "homing_3" :                        #第三步判定是否开始接近方块
                            result_1= find_nearest_box()
                            time.sleep(0.2)
                            result_2= find_nearest_box()
                            time.sleep(0.2)
                            result_3= find_nearest_box()
                            time.sleep(0.2)
                            result_4= find_nearest_box()
                            time.sleep(0.2)
                            result_5= find_nearest_box()
                            if result_1 == True and result_2 == True and result_3 == True and result_4 == True and result_5 == True :
                                result_green_over=False
                                result_orange_over=False
                                homing_level="homing_4"
                        elif homing_level == "homing_4" :                        #第四步判断接近容错率高的合适方块
                            result_green_1=find_box_green()
                            time.sleep(0.2)
                            result_green_2=find_box_green()
                            time.sleep(0.2)
                            result_green_3=find_box_green()
                            time.sleep(0.2)
                            result_green_4=find_box_green()
                            time.sleep(0.2)
                            result_green_5=find_box_green()
                            if result_green_1 == True and result_green_2 == True and result_green_3 == True and result_green_4 == True and result_green_5 == True :
                                result_green_over=True
                                chest_circle_green_x=chest_circle_x
                                chest_circle_green_y=chest_circle_y
                            result_orange_1=find_box_orange()
                            time.sleep(0.2)
                            result_orange_2=find_box_orange()
                            time.sleep(0.2)
                            result_orange_3=find_box_orange()
                            time.sleep(0.2)
                            result_orange_4=find_box_orange()
                            time.sleep(0.2)
                            result_orange_5=find_box_orange()
                            if result_orange_1 == True and result_orange_2 == True and result_orange_3 == True and result_orange_4 == True and result_orange_5 == True :
                                result_orange_over=True
                                chest_circle_orange_x=chest_circle_x
                                chest_circle_orange_y=chest_circle_y
                            if result_green_over==True and result_orange_over==False :
                                while find_box_green() :
                                    # goto_box_mini_pro('upgreen')
                                    goto_box_mini()
                                    if level=="end_box":
                                        homing_level = "homing_5"
                                        break
                            elif result_green_over==False and result_orange_over==True :
                                while find_box_orange() :
                                    # goto_box_mini_pro('uporange')
                                    goto_box_mini()
                                    if level=="end_box":
                                        homing_level = "homing_5"
                                        break
                            elif result_green_over==True and result_orange_over==True :
                                while find_box_green() :
                                    # goto_box_mini_pro('upgreen')
                                    goto_box_mini()
                                    if level=="end_box":
                                        homing_level = "homing_5"
                                        break
                        elif homing_level=="homing_5" :
                            marker = Tag.get_nearest_marker()
                            if len(marker) != 0:
                                print("抱箱找到了1号码准备前进")
                                step=1
                                ID+=1
                                break
                            else:
                                BoxL_move2(1)
            else:
                marker = Tag.get_nearest_marker()
                print("GETnearest_marker")
                if   key_case=="next":
                    print("next方案不执行找码1")
                elif key_case=="1":
                    print("case 1 靠近tag码1")
                    while len(marker) == 0 and ID==1:
                        # box_go(1)
                        Box_Back(1)
                        marker = Tag.get_nearest_marker()
                elif key_case=="2":
                    if key_ifIdentifyTag1 == "1":#方块挡住了1号码千万别用
                        print("key_ifIdentifyTag1 == 1")
                        print("case 2 准备靠近tag码1")
                        while len(marker) == 0 and ID==1:
                            print("case 2 尝试找到tag码1")
                            box_go1(1)
                            marker = Tag.get_nearest_marker()
                    if key_ifIdentifyTag1 == "2":#方块挡住一号码且初始放的位置侧移过去然后往前走能看到2号码你就用或者你希望效率高就用，当然走不到二号码也没事，会后退找到1号码再前进
                        print("case 2 略过tag码1")
                    else:#我不选择是否识别tag码1,那就啥也不执行,这种情况其实是有容错的
                        #1.假设放的很后面，看不到1号码，那么就会执行下面的看不到码直接跑8步并且ID+1,这个时候分两种情况 
                            # 1.跑了8步还在1号码之前:由于ID加1了会走1号码相应动作的else
                            # 2.跑了8步把1号码踩住,这个时候ID是2,目标是找二号码，但是找不到二号码会后退，那么刚巧会后退到1号码前面，会走1号码相应动作的else
                            # 3.跑了8步跑过1号码看到了2号码这个时候会非常完美刚好不对1号码(当放的很后面的时候基本不可能)
                        #2.假设放的很前（这个度自己测试），看到1号码，那么就会执行下面的正常识别的逻辑
                        #3.假设1号码被方块挡住了，就是找不到tag1, 那么就会执行下面的看不到码直接跑8步并且ID+1,这个时候分两种情况
                            #1.方块背踢到了2号码把而二号码挡住了，这个时候会不断后退，前进，进入死循环，裁判愿意拿走那么谢天谢地，不肯拿走直接寄走极端吧
                            #2.谢天谢地方块没有挡住那么完美成功
                        #4.唯一的一种逆天操作:放的很后面，跑了8步，1号码被方块挡住，这个时候会无限后退，出现这种情况只有在后退两三次的时候亡羊补牢（80概率走极端20概率找裁判）
                        print("case 2 准备靠近tag码1 有问题")
                elif key_case=="3":
                    print("case 3 靠近tag码1")
                    while len(marker) == 0 and ID==1:
                        BoxL_move2(1)
                        marker = Tag.get_nearest_marker()
                elif key_case=="4":
                    print("case 4 靠近tag码1")
                    while len(marker) == 0 and ID==1:
                        box_go(1)
                        marker = Tag.get_nearest_marker()
                        tag1k=1
                    if tag1k==1:
                        box_go(1)
                        tag1k=0

                if len(marker) == 0 and ID !=1:
                    print("无tag")
                    if (ID == 1 and level == "end_box"):
                        #BoxR_turn2(1)
                        Box_Back(1)
                    elif ID==4:
                        box_go(1)
                    elif (ID == 1 and level == "start_moving") or ID == 2 or (ID == 5 and step == 1):
                        Box_Back(1)
                    elif (ID == 5 and step == 2 and level == "start_moving"):
                        R_turn2(1)
                    elif (ID == 6) or (ID == 7) or (ID == 5 and step == 2 and level == "reverse_moving"):
                        Back2(1)

                elif len(marker) != 0 or ID==1:
                    print("fcsvdsvdfvdfvdvdfvdf")
                    if len(marker) != 0:
                        robot_tag_x = marker[1]
                        robot_tag_y = marker[2]
                        tag_yaw = marker[3] + 90 # artag 正方向与机器人正方向对齐
                    if step==1:
                        if (len(marker) != 0 and marker[0] == 1) or (len(marker) == 0 and ID == 1):
                            print("怀疑在这里面")
                            if len(marker)==0 or key_ifIdentifyTag1 == 2:
                                level = "start_moving"
                                box_go4(1)
                                # Box_go_slow8s(1)
                                ID += 1
                                print('略过一号码，直接前进对正二号码')
                            elif len(marker)!=0:
                                if ID == 1:
                                    level = "start_moving"
                                    if   key_case=="1":
                                        # result =turn_to_angle(tag_yaw,0,2) #turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.20 , 0.0 , 2)
                                        # result = turn_to_y(robot_tag_y,-0.03,0.04)
                                        '''result =turn_to_angle(tag_yaw,0,2) #turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.20 , 0.0 , 2)
                                        result = turn_to_y(robot_tag_y,-0.03,0.04)
                                        result = turn_to_x(robot_tag_x,0.1,0.03)'''
                                        result = True
                                    elif key_case=="2":
                                        #result = turn_to_y(robot_tag_y,0.0)
                                        #result = turn_to_angle(tag_yaw,0,2)
                                        # result =turn_to_angle(tag_yaw,0,2) #turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.20 , 0.0 , 2)
                                        # result = turn_to_y(robot_tag_y,-0.03,0.04)
                                        result =turn_to_angle(tag_yaw,0,2) #turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.20 , 0.0 , 2)
                                        result = turn_to_y(robot_tag_y,-0.03,0.04)
                                        result = turn_to_x(robot_tag_x,0.1,0.03)
                                    elif key_case=="3":
                                        result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.15 , 0.0 , 2, 0.03 , 0.07 , 2 )
                                    else:
                                        result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.15 , 0.0 , 2, 0.03 , 0.07 , 2 )
                                    if result == True:
                                        if key_case=="1":
                                            print('一号码对正完毕，前进对正二号码')
                                            #box_go4(2)
                                            box_go8(1)
                                            # Box_go_slow6s(1)
                                            ID+=1
                                        elif key_case=="2":
                                            box_go8(1)
                                            # Box_go_slow6s(1)
                                            ID+=1
                                        else:
                                            print('一号码对正完毕，前进对正二号码')
                                            ID += 1
                                            box_go3(2)
                                            # Box_go_slow8s(1)
                                else:
                                    print("1111")
                                    box_go3(1)
                                    # Box_go_slow4s(1)
                            print('怀疑在这里面——end')

                        elif marker[0] == 2:
                            if ID == 2:
                                result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.15 , 0.05 , 1 , 0.03 , 0.07 , 2 )
                                #result =turn_to_angle(tag_yaw,0,2) #turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.20 , 0.0 , 2)
                                #result = turn_to_y(robot_tag_y,-0.03,0.04)
                                if result == True:
                                    print('二号码对正完毕，右侧移对正三号码')
                                    ID += 2
                                    box_go(1)
                                    BoxR_move2(8)
                                    box_go8(1)
                            else:
                                BoxR_move2(1)

                        elif marker[0] == 3:
                            if ID == 3:
                                result =turn_to_angle( tag_yaw , 0 , 2 ) #turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.16 , 0.11 , 0)
                                result=turn_to_x(robot_tag_x,0.2,0.03)
                                if result == True:
                                    print('三号码对正完毕，右侧移对正四号码')
                                    ID += 1
                                    BoxR_move2(3)
                                    #BoxR_move1(1)
                                    box_go8(1)
                                    # box_go4(1)
                                    # box_go6(1)
                                    # Box_go_slow8s(1)
                                    #box_go1(1)
                            else:
                                BoxR_move2(1)
                                

                        elif marker[0] == 4:
                            if ID == 4:
                                result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.15 , 0 , 2)
                                if result == True:
                                    print('四号码对正完毕，左移对正五号码')
                                    ID += 1
                                    BoxL_move2(5)
                                    box_go8(1)
                                    # Box_go_slow8s(1)
                                    # Box_Down(1)
                                    Stand(1)
                                    go_fast2(1)
                                    #Body_Down(1)
                                    break
                                    while True:
                                        R_turn2(1)
                                        marker = Tag.get_nearest_marker()
                                        if len(marker) != 0:
                                            R_turn2(2)
                                            break
                                    print("结束一轮")
                                    step=2
                                   
                                    time.sleep(0.5)
                                    
                    elif step == 2:
                        #反方向
                        if marker[0] == 5:
                            result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.08 , 0.0 , 180)#5号为反方向
                            level = "reverse_moving"
                            if result == True:
                                print('五号码对正完毕，左侧移对正8号码')
                                ID += 1
                                # go_fast3(1)
                                # go_fast1(1)
                                go_fast4(1)
                        elif marker[0] == 6:
                            if ID == 6:
                                result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.05 , -0.1 , 5)
                                if result == True:
                                    print('六号码对正完毕，左侧移对正七号码')
                                    ID += 1
                                    L_move2(5)
                                    # go_fast3(2)
                                    go_fast6(1)
                        
                        elif marker[0] == 7:
                            if ID == 7:
                                result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.06 , 0.10 , -2)
                                if result == True:
                                    print('七号码对正完毕，右侧移反向对正一号码')
                                    R_move2(9)
                        elif marker[0] == 1:
                            result = turn_to_tag(robot_tag_x, robot_tag_y, tag_yaw, 0.0 , 0.07 , 180, 0.03 , 0.04 , 5 )#1号为反方向
                            if result == True:
                                print('一号码对正完毕，前进抓取海绵块')
                                # go_fast3(1)
                                go_fast1(2)
                                go_fast6(1)
                                ID = 0
                                level = "next_box"
                                homing_level = "homing_1"
                                key_case="next"

                time.sleep(0.1)
        while True:                              #-----------------------------------------确定是否重启-----------------------------------------#
            print("1 -->重启 other:停留 ")
            key_resart=input("是否重启")
            if key_resart=="1":
                ID = 0      
                step = 1
                level = "start_box"
                break

